clear all
% close all

save_plots	= 0;

fprintf('\nStiffness script START\n');
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%



n = 100;

f = logspace(0,2,n);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Compile logs

fprintf('Compiling logs\n');
f_lim = [0 0 0];

for s = 1:3
	
	log_name = sprintf('./fixedtoe/k%d_logs_100_good/test',s);
	
	% Grab system parms...
	str = sprintf('./spring%d/sys%d.mat',s,s);
	load(str);
	
	tauc	= sys.tauc;
	GR		= sys.GR;

	K		= sys.K;
	B		= sys.B;
	I		= sys.I;
	OFFSET	= sys.OFFSET;
	
	w0	= sqrt(K/I);

	z	= B/(2*sqrt(K*I));
	wd	= w0*sqrt(1-z^2);
	fd	= wd/(2*pi);

	A2T = tauc*GR;

	% Grab controller parms...
	str = sprintf('./spring%d/ctrl%d.mat',s,s);
	load(str);
	a		= ctrl.a;
	trq_lim = ctrl.trq_lim;

	
	% Open the logs and deal with the data for a spring
	for test_idx = 1:n
		fprintf('Opening log:\t');
		fprintf('%s.%d.log\n',log_name,test_idx);

		log_str = sprintf('%s.%d.log',log_name,test_idx);
		log = importdata(log_str);

		motor_angle		= log.data(:,4);
		leg_angle		= log.data(:,5);

		motor_current	= log.data(:,8);

		spring_pos = motor_angle - leg_angle + OFFSET;

		spring_trq = K*spring_pos;

		t = log.data(:,1);

		W = ceil(length(t)*0.03);
		if (W>10)
			W = 10;
		end

	% 	idx = round(500/f(test_idx));
		idx = length(t);


		mtr_trq = motor_current*A2T;

		temporary	= filter( repmat( 1/2/W, 1, W ), 1, mtr_trq( end : -1 : 1, : ) );
		filt_mtr	= filter( repmat( 1/2/W, 1, W ), 1, mtr_trq ) ...
					+  temporary( end : -1 : 1, : );

		temporary	= filter( repmat( 1/2/W, 1, W ), 1, spring_trq( end : -1 : 1, : ) );
		filt_s		= filter( repmat( 1/2/W, 1, W ), 1, spring_trq ) ...
					+  temporary( end : -1 : 1, : );


		mtr_trq		= filt_mtr;
	%  	spring_trq	= filt_s;

		amps(test_idx,s) = max(abs(spring_trq));

% 		phase(test_idx,s) = 0;

		mag(test_idx,s) = 20*log10(amps(test_idx,s)/a);		% spring magnitudes

		if (f_lim(s) == 0)
	% 		if (sum(abs(mtr_trq)>=trq_lim) >= ceil(length(t)*0.01))
			if (max(abs(mtr_trq(1:idx)))>=trq_lim)
				f_lim(s) = f(test_idx);
	% 			figure
	% 			hold on
	% 			plot([t(1) t(end)],[trq_lim trq_lim],'k--');
	% 			plot([t(1) t(end)],[-trq_lim -trq_lim],'k--');
	% 			plot(t,mtr_trq);
	% 			plot(t,spring_trq,'r');
			end
		end


	end % log foor
end % spring for



%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
h = figure;
set(h,'position',[ 100 100 800 400])

semilogx([1 100], [0 0],'k--','linewidth', 2);
hold on

semilogx([1 100], [3 3],'k--','linewidth', 1);
semilogx([1 100], [-3 -3],'k--','linewidth', 1);

% semilogx([fd fd], [-3 3],'g','linewidth', 2);
% text(fd,max(amps)- max(amps)*0.05,'f_d \rightarrow','FontSize',12, 'HorizontalAlignment', 'right');
if (f_lim(1) ~= 0)
	semilogx([f_lim(1) f_lim(1)], [min(min(mag)) 3],'g','linewidth', 2);
	str = sprintf('$%2.3f \\  Hz$',f_lim(1));
	text(f_lim(1),-25,str,'FontSize',12, 'HorizontalAlignment', 'right','interpreter','latex');
end
if (f_lim(2) ~= 0)
	semilogx([f_lim(2) f_lim(2)], [min(min(mag)) 3],'g','linewidth', 2);
	str = sprintf('$%2.3f \\  Hz$',f_lim(2));
	text(f_lim(2),-23,str,'FontSize',12, 'HorizontalAlignment', 'right','interpreter','latex');
end
if (f_lim(3) ~= 0)
	semilogx([f_lim(3) f_lim(3)], [min(min(mag)) 3],'g','linewidth', 2);
	str = sprintf('$\\ %2.3f \\  Hz$',f_lim(3));
	text(f_lim(3),-25,str,'FontSize',12, 'HorizontalAlignment', 'left','interpreter','latex');
end

pmag = semilogx(f,mag,'linewidth', 3);
axis tight

legend(pmag,'Soft', 'Medium', 'Stiff','location','southwest');

xlabel(	'\textbf{Frequency} $\left( H z \hspace{1 mm}\right)$',...
		'interpreter','latex','fontsize',16);

ylabel(	'\textbf{Magnitude} (dB )',...
		'interpreter','latex','fontsize',16);

set(gca,'Position',[ 0.10 0.15 0.85 0.8])
	
str = sprintf('/home/drl/projects/jhurst/current/Kemper_journal/images/bodeplot.pdf');
if (save_plots == 1)
	save2pdf(str);
end


% figure
% hold on
% plot([t(1) t(end)], [a a], 'k','linewidth', 2);
% plot([t(1) t(end)], [-a -a], 'k','linewidth', 2);
% plot(t,spring_trq,'linewidth', 2);
% axis tight


% figure
% hold on
% plot(t,mtr_trq,'linewidth', 2);
% axis tight


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% mail = 'kkemper@gmail.com'; %Your GMail email address
% password = '57d.alN.'; %Your GMail password
% % Don't touch unless you need to change the Email supplier (currently Gmail)
% setpref('Internet','E_mail',mail);
% setpref('Internet','SMTP_Server','smtp.gmail.com');
% setpref('Internet','SMTP_Username',mail);
% setpref('Internet','SMTP_Password',password);
% props = java.lang.System.getProperties;
% props.setProperty('mail.smtp.auth','true');
% props.setProperty('mail.smtp.socketFactory.class', 'javax.net.ssl.SSLSocketFactory');
% props.setProperty('mail.smtp.socketFactory.port','465');
% 
% sendmail('kkemper@gmail.com','matlab is done!')